/*========================================================================
			ROBOCON 2008 - PUNE INDIA
			BKIT4 - CNTT DH BACH KHOA TPHCM
			HARDWARE CLUB  AUTO ROBOT
			File : pid.c
=========================================================================*/

#include <util/delay.h>
#include "define.h"
#include "FreeRTOS_adc.h"
#include "FreeRTOS_speed.h"
#include "FreeRTOS_encoder.h"
#include "FreeRTOS_pid.h"
/*======================================*/
/* Symbol definition                    */
/*======================================*/
//old robo
int vantoc_R,vantoc_L,quang_duong,time_out;

//bien dung de dieu xung theo phuong phap 

int vantoc_R,vantoc_L,quang_duong;

void test_sensor(unsigned char sensor_bits, int *dev_, int *old_dev_)
{
	*old_dev_ = *dev_;
	switch (sensor_bits & MASK2_2)
	{
		case 0x18: //001xx100 trung tam OK//status =OK;
			*dev_ = 0;
			break;
		case 0x08://000xx100//status = LEFT_CENTER_OUT1;
			if( (sensor_bits& 0x0c) == 0x0c)
				*dev_ = 3;			//00001100
			else *dev_ = 2;			//00001000	
			break;
		case 0x10://001xx000//status = RIGHT_CENTER_OUT1;
			if( (sensor_bits& 0x30) == 0x30)
				*dev_ = -3;			//00110000
			else *dev_ = -2;			//00010000
			break;
		case 0x00:
			switch(sensor_bits&MASK3_3)
			{
				case 0x04://000xx100//status = LEFT_OUT1;
					*dev_ = 4;
					break;
				case 0x07:
				case 0x05:
				case 0x06://000xx110//status = LEFT_OUT1;
					*dev_ = 5;
					break;
				case 0x02://000xx010//status = LEFT_OUT2;
					*dev_ = 7;
					break;
				case 0x03://000xx011//status = LEFT_OUT2;
					*dev_ = 9;
					break;
				case 0x01://000xx001//status = LEFT_OUT3;
					*dev_ = 11;
					break;
				case 0x20://status = RIGHT_OUT1;
					*dev_ = -3;
					break;
				case 0x60://status = RIGHT_OUT1;
					*dev_ = -4;
					break;
				case 0x40://status = RIGHT_OUT2;
					*dev_ = -7;
					break;
				case 0xe0:
				case 0xb0:	
				case 0xc0://status = RIGHT_OUT2;
					*dev_ = -9;
					break;
				case 0x80://status = RIGHT_OUT3;
					*dev_ = -11;
					break;
				case 0x00 :
					*dev_ = 25;
					break;
				default://dev = 20;  //error
					break;
		}
	}
}

//----------------------------------------------------------------
//bo sung ham test forward pd
//input: _dev,_old_dev,stand_speed,
void calc_forward_pd(int _dev, int *_old_dev, int _stand_speed, int *l,int *r)
{
	int hieuchinh_L ; 
	int hieuchinh_R ;	
	int PWM_P ; 
	int PWM_D ;
	int Left_Speed=0, Right_Speed=0;
	//test_sensor();
	//dev = (0 - dev);
	if (_dev > -20 && _dev < 20)
	{
	 	//toc do = 50, do lech cu la 5,do lech moi la 6	   dang lech manh hon
		PWM_P = (_stand_speed * _dev) 	;	//=50x6 = 300 pwm_d = k*dev/stand_speed
		PWM_D = _stand_speed*(_dev - *_old_dev) ;//=1.50 = 50		

		hieuchinh_L = _stand_speed - ( PWM_P/Div_P + PWM_D/Div_D);	 //ko duoc am =50+300/10 +	50/8=50+30+ 5=85 = 250
		hieuchinh_R = _stand_speed + ( PWM_P/Div_P + PWM_D/Div_D);	 //ko duoc am =50-300/10 + 50/8=50-30-5=15
		 
		if (hieuchinh_L > _stand_speed) Left_Speed = _stand_speed; 
		else if (hieuchinh_L < -60) Left_Speed = -60;
		//else if (hieuchinh_L < 0) Left_Speed = -50;
		//else if (hieuchinh_L < 15) Left_Speed = 15;
		else Left_Speed = hieuchinh_L;

		if (hieuchinh_R > _stand_speed) Right_Speed = _stand_speed; 
		else if (hieuchinh_R < -60) Right_Speed = -60;
		//else if (hieuchinh_R < 15) Right_Speed = 15;
		else Right_Speed = hieuchinh_R;
		speed(Right_Speed,Left_Speed); 		  
	}
	else if (_dev == 20) 
	{
		Left_Speed = 10;
		Right_Speed = 10;
	}else{
		Left_Speed = 0;
		Right_Speed = 0;
	}
	*l = Left_Speed;
	*r = Right_Speed;
	*_old_dev = _dev;
}
//ham dung de on dinh robo tai trang thai hai led giua
//Input: dev, stable_old_dev,pos,stand_speed
int keep_stable(char pos,int *dev_,int *stable_old_dev_,char stand_speed_)
{
	int PWMacc =0;
	int Left_Speed, Right_Speed;
	//speed(100,100);
	if (*dev_ > -11 && *dev_ < 11)
	{
	 	//toc do = 50, do lech cu la 5,do lech moi la 6	   dang lech manh hon
		*dev_ = *dev_ - pos ;
		//	PWMacc =  dev*15 + (dev - stable_old_dev)*6;	 //6 ,2-1 =1->7	,
		PWMacc = *dev_*9 + (*dev_ - *stable_old_dev_)*5;	 //6 ,2-1 =1->7	,
		if(*dev_ > 0)
		{
			if(*dev_ - *stable_old_dev_ >= 1)
				*stable_old_dev_ = *dev_ - 1;
			if(*dev_ - *stable_old_dev_ < - 1)
				*stable_old_dev_ = *dev_ + 1;
		}
		else if(*dev_ < 0)
		{
			if(*dev_ - *stable_old_dev_ >= 1)
				*stable_old_dev_ = *dev_ - 1;
			if(*dev_ - *stable_old_dev_ < - 1)
				*stable_old_dev_ = *dev_ + 1;
		}
		else
			*stable_old_dev_ = *dev_ ;//cu=3	,2 ,1,0
		if (PWMacc > 80 ) PWMacc = 80; 
		else if (PWMacc < -80) PWMacc = -80;
		Right_Speed = PWMacc;
		Left_Speed =  - PWMacc;
		speed(Right_Speed,Left_Speed);
	}
	else if (*dev_ == 20) 
	{
		if(stand_speed_>=20)
		{
         	Right_Speed = stand_speed_-(4*stand_speed_)/6;
            Left_Speed = stand_speed_-(3*stand_speed_)/4;
          	speed(Right_Speed,Left_Speed); 
	 	}
		else 
		{
			speed(20,20);
		}
	}
	return PWMacc;
} 


//----------------------------------------------------------------
//PID control
static int L_acc ;	//* accumulators for motor output values */
static int L_last ; //* history values for calculating derivative */
static int R_acc ;	//* accumulators for motor output values */
static int R_last ; //* history values for calculating derivative */
/*
input:  	L_velocity, R_velocity	: requested input velocities, ticks per 1/20 sec
		velctl_L,velctl_R		: controling velocity
outout:	give the speed to motors
*/
void pd_velocity(int velctl_L, int velctl_R, int L_velocity,int R_velocity,int *L_vel_buf ,int *R_vel_buf ){
	int delta,error;
	//PID controller for Left
	error = (velctl_L - L_velocity)*128 ;	
	delta = (error - L_last);
	L_acc +=  ( error/K_PRO + delta/K_DRV);
	L_last = error; 
	if (L_acc > TMAX) L_acc = TMAX; 
	else if (L_acc < -TMAX) L_acc = -TMAX; 
	*L_vel_buf = L_acc/128;
	//PID controller for Right
	error = (velctl_R - R_velocity)*128 ;	
	delta = (error - R_last) ;
	R_acc +=  ( error/K_PRO + delta/K_DRV);
	R_last = error; 
	if (R_acc > TMAX) R_acc = TMAX; 
	else if (R_acc < -TMAX) R_acc = -TMAX; 
	*R_vel_buf = R_acc/128;
}

void reset_hold_velocity(int *velctl_L, int *velctl_R, unsigned char *hole_velocity_flag)

{
	*velctl_L = 0;
	*velctl_R = 0;
	L_last = 0;
	L_acc  = 0;
	R_last = 0;
	R_acc = 0;
	*hole_velocity_flag = 0;

}


//----------------------------------------------------------------

void faster_speed(int *_stand_speed,int _speed_max)
{	
	if(*_stand_speed < _speed_max) 
		*_stand_speed += 2;
}
//----------------------------------------------------------------
void slower_speed(int *_stand_speed,int _speed_min)
{
	if(*_stand_speed > _speed_min)
		*_stand_speed -= 2;
}
//----------------------------------------------------------------
unsigned char NumLedInLine(unsigned char sen)
{
	char i, result;
	result = 0;
	for(i = 0; i<8; i++)
	{
		if((sen>>i)&0x01) result++;
	}
	return result;
}
//----------------------------------------------------------------
void check_crossline(unsigned char _sensor,unsigned char *_cross_flag)
{
	unsigned char temp=NumLedInLine(_sensor);
	if(temp >= 4) //neu ca 3 led right cung sang thi OK
			*_cross_flag = CROSS_LINE;
	else
	if(temp == 0) 
			*_cross_flag = NO_LINE;
	else
			*_cross_flag = IS_LINE;
	
}
//----------------------------------------------------------------

